#include "../include/2Dof_Model_dyna.h"

#include "ros/ros.h"
#include "../include/serial_struct.h"
#include "../include/hardware/robot.h"
// #include "../include/eigen/Eigen/Dense"
// #include "eigen/Eigen/Dense"
#include <math.h>
#include <iostream>
#include <thread>
#include <condition_variable>
#include <sensor_msgs/Joy.h>

using std::cerr;
using std::cout;
using std::endl;
using std::exception;
using std::string;
using std::vector;
using namespace std;
using namespace Eigen;

// 过渡
void now2start(float nowq[2], float startq[2], float q1Trace[1000], float q2Trace[1000])
{
  float dltq[2] = {0}, stepq[2] = {0};
  for (size_t i = 0; i < 2; i++)
  {
    dltq[i] = startq[i] - nowq[i];
    stepq[i] = dltq[i] / 1000.0;
  }
  // cout<<"nowq1； "<<nowq[0]<<endl;
  // cout<<"startq "<<startq[0]<<endl;
  // cout<<"dltq "<<dltq[0]<<endl;
  // cout<<"stepq "<<stepq[0]<<endl;

  for (size_t i = 0; i < 1000; i++)
  {
    q1Trace[i] = nowq[0] + i * stepq[0];
    q2Trace[i] = nowq[1] + i * stepq[1];
    // cout<<"q1: "<<q1Trace[i]<<endl;
    /* code */
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "myARM");
  ros::NodeHandle n;
  ros::Rate r(1000);
  lively_robot::robot rb; // 建一个机器人，包括板子，端口以及串口的线程打开等操作
  ROS_INFO("\033[1;32mSTART\033[0m");

  // 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
  livelybot_msg::MotorState set_msg;
  ros::Publisher set_motor_pub = n.advertise<livelybot_msg::MotorState>("/livelybot_real_real/RS01set_controller/state", 1);

  // 获取电机数据
  motor_back_t joint_motor[2];

  UR2DOFARM DofModel;
  // 标准的DH参数
  float a_sd[2] = {0.1, 0.2};
  float alpha_sd[2] = {0, 0};
  float d_sd[2] = {0, 0};
  DofModel.SetDH_SD(a_sd, d_sd, alpha_sd);

  // 加载模型
  pinocchio::Model model;
  std::string model_path = "/home/koliko/ROS/arm_ws/src/2dof_urdf/urdf/2dof_urdf.urdf";
  pinocchio::urdf::buildModel(model_path, model);
  pinocchio::Data data(model);

  // 正逆运动学
  bool ik = false;
  if (ik)
  {
    /*********过渡到期望位置*********************/
    float nowMotor[2] = {0}, startMotor[2] = {0};
    float m1Trace[1000] = {0}, m2Trace[1000] = {0};

    // 获取电机当前状态
    for (size_t i = 0; i < 30; i++)
    {
      rb.Motors[1]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
      rb.Motors[0]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
      rb.motor_send();
      r.sleep();
      /* code */
    }
    joint_motor[0] = *rb.Motors[0]->get_current_motor_state();
    nowMotor[0] = joint_motor[0].position;
    joint_motor[1] = *rb.Motors[1]->get_current_motor_state();
    nowMotor[1] = joint_motor[1].position;

    now2start(nowMotor, startMotor, m1Trace, m2Trace);
    cout << "》》》正在前往目的地！！\n"
         << endl;

    for (size_t i = 0; i < 1000; i++)
    {
      // cout<<"q:"<<m1Trace[i]<<", "<<m1Trace[i]<<", "<<m1Trace[i]<<", "<<m1Trace[i]<<", "<<m1Trace[i]<<", "<<m1Trace[i]<<endl;
      rb.Motors[1]->fresh_cmd(m2Trace[i], 0.0, 0, 200.0, 0.5); // q1
      rb.motor_send();
      r.sleep();
    }
  }

  /*重力补偿和拖动示教*/
  bool g = false;
  uint32_t g_time = 20000; // 30s
  if (g)
  {
    cout << "正在进行重力补偿测试\n"
         << endl;
    // 力矩
    Vector2f tor;
    // 电机返回值
    double joint[2] = {0}, jointd[2] = {0}, jointdd[2] = {0};

    // 获取电机当前状态
    for (size_t i = 0; i < 30; i++)
    {
      rb.Motors[1]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
      rb.Motors[0]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
      rb.motor_send();
      r.sleep();
      /* code */
    }
    joint_motor[0] = *rb.Motors[0]->get_current_motor_state();
    joint_motor[1] = *rb.Motors[1]->get_current_motor_state();

    // 这里注意方向
    joint[0] = -joint_motor[0].position;
    joint[1] = joint_motor[1].position;
    jointd[0] = -joint_motor[0].velocity;
    jointd[1] = joint_motor[1].velocity;

    while (g_time--)
    // while (1)
    {
      // 设置力矩
      tor = DofModel.Get_Tor(joint, jointd, jointdd, model, data);
      ROS_INFO("%.2f\t%.2f", tor[0], tor[1]);

      // rb.Motors[0]->fresh_cmd(0, 0, 0, 0, 0); // q1
      // rb.Motors[1]->fresh_cmd(0, 0, 0, 0, 0);  // q2
      rb.Motors[0]->fresh_cmd(0, 0, -tor[0], 0, 0); // q1
      rb.Motors[1]->fresh_cmd(0, 0, tor[1], 0, 0);  // q2
      rb.motor_send();

      // 更新数据
      joint_motor[0] = *rb.Motors[0]->get_current_motor_state();
      joint_motor[1] = *rb.Motors[1]->get_current_motor_state();

      // 这里注意方向
      joint[0] = -joint_motor[0].position;
      joint[1] = joint_motor[1].position;
      jointd[0] = -joint_motor[0].velocity;
      jointd[1] = joint_motor[1].velocity;

      // ROS_INFO("%.2f\t%.2f", joint[0],joint[1]);

      // 发布
      // set_motor_pub.publish(set_msg);
      r.sleep();
    }
    cout << "测试完毕" << endl;
  }

  // 阻抗控制
  bool impedance_control = true;
  uint32_t ic_time = 20000;
  if (impedance_control)
  {
    // 2. 定义输入参数
    Eigen::Vector2d desired_end_pos(0.0, -0.20); // 期望的末端位置 (x, y)
    Eigen::Vector2d desired_end_vel(0.0, 0.0);   // 期望的末端速度 (vx, vy)
    Eigen::Vector2d current_joint_vel(0.0, 0.0); // 当前的关节速度 (dq1, dq2)
    double stiffness = 80.0;                     // 刚度系数 k
    double damping = 20.0;                       // 阻尼系数 c

    // 获取电机当前状态
    for (size_t i = 0; i < 30; i++)
    {
      rb.Motors[1]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
      rb.Motors[0]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
      rb.motor_send();
      r.sleep();
      /* code */
    }

    Eigen::Vector2d joint_torque;      // 关节力矩
    Eigen::Vector2d current_joint_pos; // 当前的关节位置 (q1, q2)

    while (ros::ok() && ic_time--)
    {
      // 获取当前位置
      joint_motor[0] = *rb.Motors[0]->get_current_motor_state();
      joint_motor[1] = *rb.Motors[1]->get_current_motor_state();
      current_joint_pos(0) = -joint_motor[0].position;
      current_joint_pos(1) = joint_motor[1].position;
      // 3. 调用阻抗控制函数
      DofModel.impedance_control_2dof(desired_end_pos, desired_end_vel, current_joint_pos, current_joint_vel, stiffness, damping, model, data, joint_torque);
      // 4. 输出结果
      std::cout << "计算得到的关节力矩 (τ1, τ2): " << joint_torque.transpose() << std::endl;

      // 电机控制
      rb.Motors[0]->fresh_cmd(0, 0, -joint_torque(0), 0, 0); // q1
      rb.Motors[1]->fresh_cmd(0, 0, joint_torque(1), 0, 0);  // q2
      // rb.Motors[0]->fresh_cmd(0, 0, 0, 0, 0); // q1
      // rb.Motors[1]->fresh_cmd(0, 0, 0, 0, 0);  // q2
      rb.motor_send();

      r.sleep();
    }
  }

  // 回零操作
  bool back_zero = true;
  uint32_t keep_time = 20000;
  if (back_zero)
  {
    /*********过渡到期望位置*********************/
    float nowMotor[2] = {0}, startMotor[2] = {0};
    float m1Trace[1000] = {0}, m2Trace[1000] = {0};
    // 获取电机当前状态
    for (size_t i = 0; i < 30; i++)
    {
      rb.Motors[1]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
      rb.Motors[0]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
      rb.motor_send();
      r.sleep();
      /* code */
    }
    joint_motor[0] = *rb.Motors[0]->get_current_motor_state();
    nowMotor[0] = joint_motor[0].position;
    joint_motor[1] = *rb.Motors[1]->get_current_motor_state();
    nowMotor[1] = joint_motor[1].position;

    now2start(nowMotor, startMotor, m1Trace, m2Trace);
    cout << "正在归位\n"
         << endl;

    for (size_t i = 0; i < 1000; i++)
    {
      // cout<<"q:"<<m1Trace[i]<<", "<<m1Trace[i]<<", "<<m1Trace[i]<<", "<<m1Trace[i]<<", "<<m1Trace[i]<<", "<<m1Trace[i]<<endl;
      rb.Motors[1]->fresh_cmd(m2Trace[i], 0.0, 0, 200.0, 0.5); // q1
      rb.Motors[0]->fresh_cmd(m1Trace[i], 0.0, 0, 200.0, 0.9); // q0

      rb.motor_send();
      r.sleep();
    }
  }

  // 延时
  uint16_t Delay_time = 1000;
  while (Delay_time--)
  {
    r.sleep();
    /* code */
  }

  // 获取电机当前状态
  while (ros::ok())
  {
    // 断电
    rb.Motors[0]->fresh_cmd(0, 0, 0, 0, 0);
    rb.Motors[1]->fresh_cmd(0, 0, 0, 0, 0);
    rb.motor_send();
    r.sleep();
    /* code */
  }

  // 释放
  for (auto &thread : rb.ser_recv_threads)
  {
    thread.join();
  }
}
